﻿using Arction.Wpf.SignalProcessing;
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;

namespace WPF_UI_Lightningchart.Helper
{
    public class RealtimeFFTCalculator
    {
        /// <summary>
        /// Interval between FFT calculations.
        /// </summary>
        private double _intervalMs = 1;

        /// <summary>
        /// Sampling frequecy.
        /// </summary>
        private int _samplingFrequency = 1;

        /// <summary>
        /// First FFT data out.
        /// </summary>
        private long _startTicks = 0;

        /// <summary>
        /// Last FFT data out.
        /// </summary>
        private long _lastTicks = 0;

        /// <summary>
        /// TimeSpan interval between FFT calculations in ms.
        /// </summary>
        private long _updateInterval = 0;

        /// <summary>
        /// Amount of channels in discrete input signal.
        /// </summary>
        private int _channelCount = 0;

        /// <summary>
        /// Old discrete data was rendered.
        /// </summary>
        private double[][] _oldData;

        /// <summary>
        /// Data resolution to be presented in a monitor.
        /// </summary>
        private int _windowResolution = 512;

        /// <summary>
        /// Index of a data.
        /// </summary>
        private int _FFTEntryIndex = 0;

        /// <summary>
        /// Spectrum calculator. FFT routines with a given signal.
        /// </summary>
        private SpectrumCalculator _spectrumCalculator;

        /// <summary>
        /// FFT calculator constructor.
        /// </summary>
        /// <param name="updateIntervalMs">How often FFT should be calculated</param>
        /// <param name="samplingFrequency">Sampling frequency</param>
        /// <param name="windowLength">FFT window length. Does not have to be power of 2.</param>
        public RealtimeFFTCalculator(double updateIntervalMs, int samplingFrequency, int windowLength, int channelCount)
        {
            _channelCount = channelCount;
            _intervalMs = updateIntervalMs;
            _samplingFrequency = samplingFrequency;
            _updateInterval = TimeSpan.FromMilliseconds(updateIntervalMs).Ticks;
            _windowResolution = windowLength;
            _lastTicks = _startTicks = DateTime.Now.Ticks;

            _oldData = new double[channelCount][];
            for (int i = 0; i < channelCount; i++)
            {
                _oldData[i] = new double[0];
            }

            _spectrumCalculator = new SpectrumCalculator();
        }
        /// <summary>
        /// 加窗
        /// </summary>
        /// <param name="inputSignal"></param>
        /// <returns></returns>
        private double[] WinCal(double[] inputSignal)
        {
            double[] window = MathNet.Numerics.Window.Hann(inputSignal.Length);
            double[] windowedSignal = new double[inputSignal.Length];
            for (int i = 0; i < inputSignal.Length; i++)
            {
                windowedSignal[i] = 2 * inputSignal[i] * window[i];
            }
            return windowedSignal;
        }

        /// <summary>
        /// Calculates FFT from multi-channel sample stream.
        /// </summary>
        /// <param name="data">Sample data stream</param>
        /// <param name="xValues">X values, by channels</param>
        /// <param name="yValues">Y values, by channels</param>
        /// <returns>True if FFT result available</returns>
        public bool FeedDataAndCalculate(double[][] data, out double[][][] xValues, out double[][][] yValues)
        {
            xValues = null;
            yValues = null;

            // Validation of discrete data.
            if (data == null)
            {
                return false;
            }

            // Reference to member variable channel counter.
            int channelCounter = _channelCount;

            // Timer ticks.
            long ticksNow = DateTime.Now.Ticks;

            // Flag allows data to be rendered.
            bool giveDataOut = false;

            // 一次更新点数：Count how many samples should be rendered per update.
            int samplesPerUpdate = (int)(_intervalMs * _samplingFrequency / 1000.0);

            // 更新轮数：Amount of repeated rounds in a loop for FFT data output. Samples length / samples per update.
            int repeatFFT;

            if (_FFTEntryIndex == 0)
            {
                repeatFFT = 1;
            }
            else
            {
                repeatFFT = (int)Math.Ceiling((double)data[0].Length / samplesPerUpdate);//Ceiling向上取整
            }

            // Dimensions: FFT calculations, channel, FFT result value.
            double[][][] valuesX = new double[repeatFFT][][];
            double[][][] valuesY = new double[repeatFFT][][];

            for (int i = 0; i < repeatFFT; i++)
            {
                // 本轮更新加入的新数据长度：Calculate amount of FFT calculus points from DATA must be copied (<=samplesPerUpdate).
                int samplesCopiesAmount = Math.Min(samplesPerUpdate, data[0].Length - i * samplesPerUpdate);

                // For the 1st entry window resolution = number of points.
                if (_FFTEntryIndex == 0)
                {
                    samplesCopiesAmount = data[0].Length;
                }

                // Allocate buffers according to amount of channels.
                valuesX[i] = new double[channelCounter][];
                valuesY[i] = new double[channelCounter][];

                // Keeps information about samples amount in combined buffer.
                int samplesInCombinedData = 0;

                //System.Threading.Tasks.Parallel.For(0, channelCounter, channelNumber =>
                //{
                for (int channelNumber = 0; channelNumber < channelCounter; channelNumber++)
                {
                    // Create extended buffer with old and new data.
                    double[] combinedData = new double[_oldData[channelNumber].Length + samplesCopiesAmount];

                    // Copy OLD data.
                    Array.Copy(_oldData[channelNumber], 0, combinedData, 0, _oldData[channelNumber].Length);

                    // Copy NEW data.
                    Array.Copy(data[channelNumber], i * samplesPerUpdate, combinedData, _oldData[channelNumber].Length, samplesCopiesAmount);

                    // 可计算点数长度：Get information how many samples now.
                    samplesInCombinedData = combinedData.Length;

                    // Initiate FFT calculus at fixed interval from Time-reference
                    if (samplesInCombinedData >= _windowResolution &&
                        ticksNow >= (_startTicks + _updateInterval * (_FFTEntryIndex + 1)))
                    {
                        // Data will be visible with defined resolution.
                        double[] dataCombined = new double[_windowResolution];
                        int index = 0;
                        //取出相应点数的数据，
                        for (int j = samplesInCombinedData - _windowResolution; j < samplesInCombinedData; j++)
                        {
                            dataCombined[index++] = combinedData[j];
                        }
                        // 计算FFT数据：Calculate FFT spectrum data.
                        double[] fftResult;

                        //dataCombined = WinCal(dataCombined);
                        fftResult = fft(dataCombined, _windowResolution);

                        // FFT data quantity.
                        int length = fftResult.Length;

                        // Allocate buffers for FFT data for each channels.
                        valuesX[i][channelNumber] = new double[length];
                        valuesY[i][channelNumber] = new double[length];

                        // Period. Accurate estimate of DC (0 HZ) component.
                        double stepX = (double)_samplingFrequency / (double)(length - 1.0);

                        for (int point = 1; point < length; point++)
                        {
                            valuesX[i][channelNumber][point] = point * stepX;
                            valuesY[i][channelNumber][point] = fftResult[point] * 2f / length;
                        }

                        // Amount of new samples will be old data samples quantity in the next round.
                        int samplesAmountNew = samplesPerUpdate;

                        if (_windowResolution > samplesAmountNew)
                        {
                            samplesAmountNew = _windowResolution;
                        }

                        if (samplesInCombinedData > samplesAmountNew)
                        {
                            //Drop oldest samples out
                            _oldData[channelNumber] = new double[samplesAmountNew];
                            Array.Copy(combinedData, samplesInCombinedData - samplesAmountNew, _oldData[channelNumber], 0, samplesAmountNew);
                        }

                        giveDataOut = true;
                    }
                    else
                    {
                        _oldData[channelNumber] = combinedData;
                    }
                }
                //});

                // Avoid some uncertainties in a loop.
                if (samplesInCombinedData >= _windowResolution &&
                    ticksNow >= (_startTicks + _updateInterval * (_FFTEntryIndex + 1)))
                {
                    // Start tick counting when first entry comes.
                    if (_FFTEntryIndex == 0)
                    {
                        _startTicks = ticksNow;
                    }

                    _FFTEntryIndex++;
                }
            }

            if (giveDataOut)
            {
                _lastTicks = ticksNow;

                // Reduce FFT calculus rounds if it was not enough points to Calculate FFT.
                while (repeatFFT > 0 && valuesX[repeatFFT - 1][0] == null)
                {
                    repeatFFT--;
                }

                if (repeatFFT == 0)
                {
                    giveDataOut = false;
                    return giveDataOut;
                }

                xValues = new double[repeatFFT][][];
                yValues = new double[repeatFFT][][];

                for (int i = 0; i < repeatFFT; i++)
                {
                    xValues[i] = new double[channelCounter][];
                    yValues[i] = new double[channelCounter][];

                    // Copy FFT results to output.
                    for (int channelNumber = 0; channelNumber < channelCounter; channelNumber++)
                    {
                        xValues[i][channelNumber] = valuesX[i][channelNumber];
                        yValues[i][channelNumber] = valuesY[i][channelNumber];
                    }
                }
            }

            return giveDataOut;
        }

        public void Dispose()
        {
            if (_spectrumCalculator != null)
            {
                _spectrumCalculator.Dispose();
                _spectrumCalculator = null;
            }
        }

        private int m;

        private double[] cos;

        private double[] sin;
        private double[] fft(double[] Samples, int SampleNum)
        {
            cos = new double[SampleNum / 2];
            sin = new double[SampleNum / 2];
            double[] fftout = new double[SampleNum];

            this.m = (int)(Math.Log(SampleNum) / Math.Log(2));
            int i;
            for (i = 0; i < SampleNum / 2; i++)
            {
                cos[i] = Math.Cos(-2 * Math.PI * i / SampleNum);
                sin[i] = Math.Sin(-2 * Math.PI * i / SampleNum);
            }
            int j, k, n1, n2, a;
            double c, s, t1, t2;
            double[] Imag = new double[SampleNum];
            // Bit-reverse
            j = 0;
            n2 = SampleNum / 2;
            for (i = 1; i < SampleNum - 1; i++)
            {
                n1 = n2;
                while (j >= n1)
                {
                    j = j - n1;
                    n1 = n1 / 2;
                }
                j = j + n1;

                if (i < j)
                {
                    t1 = Samples[i];
                    Samples[i] = Samples[j];
                    Samples[j] = t1;
                    t1 = Imag[i];
                    Imag[i] = Imag[j];
                    Imag[j] = t1;
                }
            }

            // FFT
            n1 = 0;
            n2 = 1;

            for (i = 0; i < m; i++)
            {
                n1 = n2;
                n2 = n2 + n2;
                a = 0;

                for (j = 0; j < n1; j++)
                {
                    c = cos[a];
                    s = sin[a];
                    a += 1 << (m - i - 1);

                    for (k = j; k < SampleNum; k = k + n2)
                    {
                        t1 = c * Samples[k + n1] - s * Imag[k + n1];
                        t2 = s * Samples[k + n1] + c * Imag[k + n1];
                        Samples[k + n1] = Samples[k] - t1;
                        Imag[k + n1] = Imag[k] - t2;
                        Samples[k] = Samples[k] + t1;
                        Imag[k] = Imag[k] + t2;
                    }
                }
            }
            for (i = 0; i < SampleNum; i++)
            {
                fftout[i] = Math.Sqrt(Samples[i] * Samples[i] + Imag[i] * Imag[i]);
            }
            return fftout;
        }
    }
}
